/**
 * COPYRIGHT NOTICE Copyright (C) 2017, Buffalo Robot Tech.co, Ltd All rights
 * reserved.
 *
 * @file cubic_spline.c
 * @brief      This is the PVT spline trajectory implementation file
 *
 * @author     chauby 
 * @email    chaubyZou@163.com
 * @date       Dec.22, 2017
 * @version    Rehab v1.0.0.0
 */
#include <string.h>
#include "cubic_spline_config.h"
#include "cubic_spline.h"

/**
 * @brief initialize the cubic spline
 * @details [long description]
 * 
 * @param p_cubic_spline [CubicSpline struct to be initialized]
 * @param name [the name]
 */
void initializeCubicSpline(CubicSpline *p_cubic_spline, const char name[32])
{
    if(NULL == p_cubic_spline)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("initialize cubic spline error, the p_cubic_spline can not be NULL\n");
        #endif
        return;
    }

    strcpy(p_cubic_spline->name, name);
    p_cubic_spline->pvt_point_num = 0;

    unsigned int index = 0;
    for(index=0; index<MAX_PVT_POINT_NUM; index++)
    {
        memset(p_cubic_spline->pvt_point_queue+index, 0, sizeof(p_cubic_spline->pvt_point_queue+index));
        memset(p_cubic_spline->pvt_param_queue+index, 0, sizeof(p_cubic_spline->pvt_param_queue+index));
    }
}

/**
 * @brief set the cubic spline's pvt point queue
 * @details [long description]
 * 
 * @param p_cubic_spline [CubicSpline struct to be added]
 * @param pvt_point_queue [the queue tobe added to cubic spline strcut]
 * @param pvt_point_num [the point number]
 */
void setCubicSplineGivenPointQueue(CubicSpline *p_cubic_spline, const PVTPoint *pvt_point_queue, const unsigned int pvt_point_num)
{
    if(NULL == p_cubic_spline)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("set cubic spline given point error, the p_cubic_spline can not be NULL\n");
        #endif
        return;
    }

    if(NULL == pvt_point_queue)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("set %s's given point queue error, the pvt_point_queue can not be NULL\n", p_cubic_spline->name);
        #endif
        return;
    }

    if(pvt_point_num < 0)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("set %s's given point queue error, the pvt_point_num < 0\n", p_cubic_spline->name);
        #endif
        return;
    }
    else if(pvt_point_num > MAX_PVT_POINT_NUM)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("set %s's given point queue error, the pvt_point_num is more than the max value\n", p_cubic_spline->name);
        #endif
        return;
    }

    unsigned int index = 0;
    p_cubic_spline->pvt_point_num = 0; ///< clear the last point num
    for(index=0; index<pvt_point_num; index++)
    {
        p_cubic_spline->pvt_point_queue[index].position = pvt_point_queue[index].position;
        p_cubic_spline->pvt_point_queue[index].velocity = pvt_point_queue[index].velocity;
        p_cubic_spline->pvt_point_queue[index].time = pvt_point_queue[index].time;
        p_cubic_spline->pvt_point_num++;
    }

    // clear the rest pvt point in queue
    for(; index < MAX_PVT_POINT_NUM; index++)
    {
        memset(p_cubic_spline->pvt_point_queue+index, 0, sizeof(p_cubic_spline->pvt_point_queue+index));
    }
}

/**
 * @brief add new given point to cubic spline's point queue 
 * @details [long description]
 * 
 * @param p_cubic_spline [the cubic spline struct to be added]
 * @param new_pvt_point [new pvt point]
 */
void addPointToCubicSplineQueue(CubicSpline *p_cubic_spline, const PVTPoint new_pvt_point)
{
    if(NULL == p_cubic_spline)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("add point to cubic spline queue error, the p_cubic_spline can not be NULL\n");
        #endif
        return;
    }

    if(p_cubic_spline->pvt_point_num >= MAX_PVT_POINT_NUM)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("add point to %s's pvt point queue error, the queue is full\n", p_cubic_spline->name);
        #endif
        return;
    }

    double t = new_pvt_point.time;
    if(t < 0)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("add point to %s's pvt point queue error, the new point's time is negetive\n", p_cubic_spline->name);
        #endif
        return;
    }

    if(p_cubic_spline->pvt_point_num <= 1) ///< there is no enough points in queue
    {
        p_cubic_spline->pvt_point_queue[p_cubic_spline->pvt_point_num].position = new_pvt_point.position;
        p_cubic_spline->pvt_point_queue[p_cubic_spline->pvt_point_num].velocity = new_pvt_point.velocity;
        p_cubic_spline->pvt_point_queue[p_cubic_spline->pvt_point_num].time = new_pvt_point.time;
        p_cubic_spline->pvt_point_num++;

        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("add point to %s's pvt point queue, now we have %d points in queue\n", p_cubic_spline->name, p_cubic_spline->pvt_point_num);
        #endif

        if(p_cubic_spline->pvt_point_num == 2) ///< update the spline trajectory params
        {
            generateCubicSplineTrajectory(p_cubic_spline);
        }
        return;
    }

    /**
    *   insert the new point into queue by it's time
    */
    unsigned int index = 0;
    unsigned int pvt_point_num = p_cubic_spline->pvt_point_num;
    PVTPoint *p_current_point = p_cubic_spline->pvt_point_queue;
    for(index=0; index < pvt_point_num; index++)
    {
        p_current_point = p_cubic_spline->pvt_point_queue + index;
        if(t < p_current_point->time) ///< new point is before current point
        {
            unsigned int last_point_index = pvt_point_num-1; ///< the last point in queue
            for(; last_point_index >= index; last_point_index--) ///< move all the points after current point to next position
            {
                p_cubic_spline->pvt_point_queue[last_point_index+1].position = p_cubic_spline->pvt_point_queue[last_point_index].position;
                p_cubic_spline->pvt_point_queue[last_point_index+1].velocity = p_cubic_spline->pvt_point_queue[last_point_index].velocity;
                p_cubic_spline->pvt_point_queue[last_point_index+1].time = p_cubic_spline->pvt_point_queue[last_point_index].time;
            }

            p_cubic_spline->pvt_point_queue[index].position = new_pvt_point.position;
            p_cubic_spline->pvt_point_queue[index].velocity = new_pvt_point.velocity;
            p_cubic_spline->pvt_point_queue[index].time = new_pvt_point.time;
            p_cubic_spline->pvt_point_num++;

            #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
            printf("add point to %s's pvt point queue, now we have %d points in queue\n", p_cubic_spline->name, p_cubic_spline->pvt_point_num);
            #endif

            return;
        }
    }

    /**
     * now, the new point will be the last point of trajectory
     */
    p_cubic_spline->pvt_point_queue[index].position = new_pvt_point.position;
    p_cubic_spline->pvt_point_queue[index].velocity = new_pvt_point.velocity;
    p_cubic_spline->pvt_point_queue[index].time = new_pvt_point.time;
    p_cubic_spline->pvt_point_num++;

    #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
    printf("add point to %s's pvt point queue, now we have %d points in queue\n", p_cubic_spline->name, p_cubic_spline->pvt_point_num);
    #endif

    /**
     * update the cubic spline trajectory
     */
    generateCubicSplineTrajectory(p_cubic_spline);
}

/**
 * @brief generate spline trajectory with two points
 * @details [long description]
 * 
 * @param start_point [the first point]
 * @param end_point [the next point]
 * 
 * @return [the PVT's params]
 */
PVTPolynomialParam generateSplineTrajectoryWithTwoPoints(PVTPoint start_point, PVTPoint end_point)
{
    float P0 = start_point.position;
    float V0 = start_point.velocity;
    float T0 = start_point.time;

    float P1 = end_point.position;
    float V1 = end_point.velocity;
    float T1 = end_point.time;

    PVTPolynomialParam pvt_param = {0};

    float T = T1 - T0;
    if(T <= 0)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("generateSplineTrajectoryWithTwoPoints error, t1=%f, is smaller than t0=%f\n", T1, T0);
        #endif
        return pvt_param;
    }

    pvt_param.d = P0;
    pvt_param.c = V0;
    pvt_param.b = (3*P1 - V1*T - 2*V0*T - 3*P0)/(T*T);
    pvt_param.a = (-2*P1 + V1*T + V0*T + 2*P0)/(T*T*T);

    return pvt_param;
}

/**
 * @brief generate the cubic spline trajectory params with pvt point queue
 * @details [long description]
 * 
 * @param p_cubic_spline [the cubic spline struct]
 * @return [TRUE: generate successful, FALSE:generate failed]
 */
unsigned int generateCubicSplineTrajectory(CubicSpline *p_cubic_spline)
{
    if(NULL == p_cubic_spline)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("update cubic spline error, the p_cubic_spline can not be NULL\n");
        #endif
        return FALSE;
    }

    unsigned int pvt_point_num = p_cubic_spline->pvt_point_num;

    if(pvt_point_num <= 1)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("update spline trajectory of %s error, there is no enough points to spline, more than 1 point needed\n", p_cubic_spline->name);
        #endif
        return FALSE;
    }

    unsigned int index = 0;
    PVTPoint *p_current_point;
    PVTPoint *p_next_point;
    PVTPolynomialParam pvt_param = {0};

    /**
     * generate all params between each neigbourhood points
     */
    for(index=0; index < pvt_point_num - 1; index++)
    {
        p_current_point = p_cubic_spline->pvt_point_queue + index;
        p_next_point = p_cubic_spline->pvt_point_queue + index + 1;
        pvt_param = generateSplineTrajectoryWithTwoPoints(*p_current_point, *p_next_point);

        p_cubic_spline->pvt_param_queue[index].a = pvt_param.a;
        p_cubic_spline->pvt_param_queue[index].b = pvt_param.b;
        p_cubic_spline->pvt_param_queue[index].c = pvt_param.c;
        p_cubic_spline->pvt_param_queue[index].d = pvt_param.d;
    }

    return TRUE;
}

/**
 * @brief get the spline trajectory value with time
 * @details [if the time is before the first point's time, it's PVT value will be same 
 *           with the first point's value, and if the time is after the last point's time,
 *           it's value will be same with the last point's value]
 * 
 * @param p_cubic_spline [description]
 * @param time [time]
 * 
 * @return [the PVT typed value]
 */
PVTPoint getCubicSplineValue(CubicSpline *p_cubic_spline, float time)
{
    PVTPoint pvt_point = {0};
    float t = time;

    if(NULL == p_cubic_spline)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("get cubic spline value error, the p_cubic_spline can not be NULL\n");
        #endif
        return pvt_point;
    }

    unsigned int index = 0;
    float a = 0;
    float b = 0;
    float c = 0;
    float d = 0;
    float t0 = 0;
    unsigned int pvt_point_num = p_cubic_spline->pvt_point_num;

    if(t < p_cubic_spline->pvt_point_queue[0].time) ///< the time is befpre the first point time
    {
        pvt_point.position = p_cubic_spline->pvt_point_queue[0].position;
        pvt_point.velocity = p_cubic_spline->pvt_point_queue[0].velocity;
        pvt_point.time = t;
        return pvt_point;
    }
    else if(t > p_cubic_spline->pvt_point_queue[pvt_point_num-1].time) ///< time is after the last point time
    {
        pvt_point.position = p_cubic_spline->pvt_point_queue[pvt_point_num-1].position;
        pvt_point.velocity = p_cubic_spline->pvt_point_queue[pvt_point_num-1].velocity;
        pvt_point.time = t;
        return pvt_point;
    }
    else ///< time is between t0 and t_end
    {
        for(index=1; index<p_cubic_spline->pvt_point_num;index++)
        {
            if(t < p_cubic_spline->pvt_point_queue[index].time)
            {
                a = p_cubic_spline->pvt_param_queue[index - 1].a;
                b = p_cubic_spline->pvt_param_queue[index - 1].b;
                c = p_cubic_spline->pvt_param_queue[index - 1].c;
                d = p_cubic_spline->pvt_param_queue[index - 1].d;
                t0 = p_cubic_spline->pvt_point_queue[index - 1].time;

                pvt_point.position = a*(t - t0)*(t - t0)*(t - t0) + b*(t - t0)*(t - t0) + c*(t - t0) + d;
                pvt_point.velocity = 3*a*(t - t0)*(t - t0) + 2*b*(t - t0) + c;
                pvt_point.time = t;

                return pvt_point;
            }
        }
    }
}

/**
 * @brief clear the pvt point queue and the pvt param queue of cubic spline
 * @details [long description]
 * 
 * @param p_cubic_spline [the cubic spline to be cleared]
 */
void clearPVTPointQueue(CubicSpline *p_cubic_spline)
{
    if(NULL == p_cubic_spline)
    {
        #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
        printf("clear cubic spline value error, the p_cubic_spline can not be NULL\n");
        #endif
        return;
    }

    p_cubic_spline->pvt_point_num = 0;

    unsigned int index = 0;
    for(index=0; index<MAX_PVT_POINT_NUM; index++)
    {
        memset(p_cubic_spline->pvt_point_queue+index, 0, sizeof(p_cubic_spline->pvt_point_queue+index));
        memset(p_cubic_spline->pvt_param_queue+index, 0, sizeof(p_cubic_spline->pvt_param_queue+index));
    }
}

/**
 * @brief show all the pvt point value in point queue of cubic spline 
 * @details [long description]
 * 
 * @param p_cubic_spline [description]
 */
void showCubicSplineQueue(CubicSpline *p_cubic_spline)
{
    #ifdef CUBIC_SPLINE_STD_IN_OUT_ENABLE
    if(NULL == p_cubic_spline)
    {
        printf("show cubic spline queue error, the p_cubic_spline can not be NULL\n");
        return;
    }

    printf("\nshow cubic spline, name:%s\n", p_cubic_spline->name);
    unsigned int index = 0;
    for(index=0; index < p_cubic_spline->pvt_point_num; index++)
    {
        printf("------index: %d\n", index);
        printf("time=%f ", p_cubic_spline->pvt_point_queue[index].time);
        printf("position=%f ", p_cubic_spline->pvt_point_queue[index].position);
        printf("velocity=%f\n", p_cubic_spline->pvt_point_queue[index].velocity);

        printf("a=%f ", p_cubic_spline->pvt_param_queue[index].a);
        printf("b=%f ", p_cubic_spline->pvt_param_queue[index].b);
        printf("c=%f ", p_cubic_spline->pvt_param_queue[index].c);
        printf("d=%f\n", p_cubic_spline->pvt_param_queue[index].d);
    }

    #endif
}